#include "NavThread.h"
#include "stdafx.h"
#include <fstream>
#include <sstream>
using namespace std;

bool CNavThread::Init()
{
	m_ofs.open("./result.txt", ios::app);
	if (!m_ofs.is_open())
		return false;
	LoadFile();
	return false;
}

bool CNavThread::Loop()
{
	Sleep(500);
	handleUpdate(500);
	return true;
}

bool CNavThread::OnExit()
{
	if (m_ofs.is_open())
	{
		m_ofs.close();
	}
	return true;
}

void CNavThread::LoadFile()
{
	if (m_pNavHandle == nullptr)
	{
		m_pNavHandle = new CNavMeshHandle();
	}
	bool bsucc = m_pNavHandle->LoadFile(1, m_geomPath, m_meshPath);
	if (bsucc)
	{
		tileStruct tile;
		if (m_pNavHandle->GetData(1, &m_navMesh, &m_navQuery, &m_geomData, tile))
		{
			m_tileCache = tile.pTile;
			initNavMesh();

		}
	}
}

void CNavThread::initNavMesh()
{
	if (m_NavMeshCrowd == nullptr)
		m_NavMeshCrowd = new CNavmeshCrowd();
	if ((m_navMesh != nullptr) && (m_navQuery != nullptr))
	{
		m_NavMeshCrowd->init(0.6f, m_navMesh, m_navQuery);
		// Make polygons with 'disabled' flag invalid.
		m_NavMeshCrowd->getEditableFilter(0)->setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);
		dtObstacleAvoidanceParams params;
		memcpy(&params, m_NavMeshCrowd->getObstacleAvoidanceParams(0), sizeof(dtObstacleAvoidanceParams));
		// Low (11)
		params.velBias = 0.5f;
		params.adaptiveDivs = 5;
		params.adaptiveRings = 2;
		params.adaptiveDepth = 1;
		m_NavMeshCrowd->setObstacleAvoidanceParams(0, &params);
		// Medium (22)
		params.velBias = 0.5f;
		params.adaptiveDivs = 5;
		params.adaptiveRings = 2;
		params.adaptiveDepth = 2;
		m_NavMeshCrowd->setObstacleAvoidanceParams(1, &params);
		// Good (45)
		params.velBias = 0.5f;
		params.adaptiveDivs = 7;
		params.adaptiveRings = 2;
		params.adaptiveDepth = 3;
		m_NavMeshCrowd->setObstacleAvoidanceParams(2, &params);
		// High (66)
		params.velBias = 0.5f;
		params.adaptiveDivs = 7;
		params.adaptiveRings = 3;
		params.adaptiveDepth = 3;
		m_NavMeshCrowd->setObstacleAvoidanceParams(3, &params);
		TestAddAgent();
	}

}

void CNavThread::TestAddAgent()
{
	if (m_NavMeshCrowd != nullptr)
	{
		CMonster *pM = new CMonster();
		MONSTER_INFO info;
		memset(&info, 0, sizeof(MONSTER_INFO));
		info.idx = 0;
		info.radius = 0.6f;
		info.height = 2.0f;
		info.maxAcceleration = 8.0f;
		info.maxSpeed = 3.5f;
		info.updateFlags = DT_CROWD_ANTICIPATE_TURNS | DT_CROWD_OPTIMIZE_VIS | DT_CROWD_OPTIMIZE_TOPO | DT_CROWD_OBSTACLE_AVOIDANCE;
		info.obstacleAvoidanceType = 0;
		info.separationWeight = 3.0f;
		info.queryFilterType = 0;
		info.modelId = 5;
		info.bObstacle = false;
		info.bCollisions = true;
		float origion[3] = { 20.64543f, 0.0f, -107.5341f };
		info.bornPos.x = origion[0];
		info.bornPos.y = origion[1];
		info.bornPos.z = origion[2];
		pM->Init(info);
		bool bsuccAdd = m_NavMeshCrowd->addAgent(pM);
		if (bsuccAdd)
		{
			TestSetTarget(info.idx);
		}
	}

}

void CNavThread::TestSetTarget(int idx)
{
	float targetPos[3] = { 46.19549f, 0.0f, -168.7159f };
	m_targetRef = 0;
	float nearestPos[3] = { 0, 0, 0 };
	const dtQueryFilter* filter = m_NavMeshCrowd->getFilter(0);
	const float* ext = m_NavMeshCrowd->getQueryExtents();
	m_navQuery->findNearestPoly(targetPos, ext, filter, &m_targetRef, nearestPos);
	bool bsuccTarget = m_NavMeshCrowd->SetMoveTarget(idx, m_targetRef, nearestPos);
	dtVcopy(m_targetPos, nearestPos);
}

void CNavThread::handleUpdate(const float dt)
{
	if (!m_navMesh)
		return;
	if (!m_tileCache)
		return;
	if (m_NavMeshCrowd != nullptr)
	{
		m_NavMeshCrowd->update(dt);
		// Update agent trails
		for (int i = 0; i < m_NavMeshCrowd->getAgentCount(); ++i)
		{
			const dtCrowdAgent* ag = m_NavMeshCrowd->getAgent(i);
			stringstream ss;
			ss << "ag npos:x=" << ag->npos[0] << ",y=" << ag->npos[1] << ",z=" << ag->npos[2] << endl;
			LogFile(ss);
		}
	}
}

void CNavThread::LogFile(stringstream &ss)
{
	m_ofs << ss.str() << endl;
}
//end